﻿#pragma once
#include <AsCvDefine.h>
#include <random>

namespace AsCv {
	/**
	 * @desc 获取maxCount个随机元素
	 */
	template<typename TYPE>
	std::vector<TYPE> getRandomElementsImpl(const std::vector<TYPE>& v, int maxCount) {
		std::random_device rd;
		std::mt19937_64 eng(rd());

		std::vector<TYPE> result;
		for (int i = 0; i < maxCount; ++i) {
			std::uniform_int_distribution<unsigned long long> distr1(0, v.size() - 1);
			int sample_index1 = distr1(eng);
			result.push_back(v.at(sample_index1));
		}
		return result;
	}

	/**
	 * @desc 计算经过两点的直线
	 */
	template<typename TYPE>
	Line getLineImpl(const cv::Point_<TYPE>& pt1, const cv::Point_<TYPE>& pt2)
	{
		Line line;
		if ((pt2 - pt1).dot(pt2 - pt1) > ZERO_VALUE)
		{
			line.a = -(pt2 - pt1).y;
			line.b = (pt2 - pt1).x;
			line.c = -(line.a*pt1.x + line.b*pt1.y);
		}
		else {
			//std::cout << "Get line failed!You pass the same point"<<std::endl;
		}
		return line;
	}

	/**
	 * @desc 圆拟合
	 */
	template<typename TYPE>
	int getCircleImpl(Circle& circle,const std::vector<cv::Point_<TYPE>>& pts)
	{
		if (pts.size() < 3) return -1;
		double sumX = 0.0;
		double sumY = 0.0;
		double sumX2 = 0.0;
		double sumY2 = 0.0;
		double sumX3 = 0.0;
		double sumY3 = 0.0;
		double sumXY = 0.0;
		double sumX1Y2 = 0.0;
		double sumX2Y1 = 0.0;
		const float N = (float)pts.size();
		for (uint i = 0; i < pts.size(); ++i)
		{
			double x = pts.at(i).x;
			double y = pts.at(i).y;
			double x2 = x * x;
			double y2 = y * y;
			double x3 = x2 * x;
			double y3 = y2 * y;
			double xy = x * y;
			double x1y2 = x * y2;
			double x2y1 = x2 * y;

			sumX += x;
			sumY += y;
			sumX2 += x2;
			sumY2 += y2;
			sumX3 += x3;
			sumY3 += y3;
			sumXY += xy;
			sumX1Y2 += x1y2;
			sumX2Y1 += x2y1;
		}

		double C = N * sumX2 - sumX * sumX;
		double D = N * sumXY - sumX * sumY;
		double E = N * sumX3 + N * sumX1Y2 - (sumX2 + sumY2) * sumX;
		double G = N * sumY2 - sumY * sumY;
		double H = N * sumX2Y1 + N * sumY3 - (sumX2 + sumY2) * sumY;

		double denominator = C * G - D * D;
		if (std::abs(denominator) < DBL_EPSILON) return -1;
		double a = (H * D - E * G) / (denominator);
		denominator = D * D - G * C;
		if (std::abs(denominator) < DBL_EPSILON) return -1;
		double b = (H * C - E * D) / (denominator);
		double c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;

		circle.centerX = a / (-2);
		circle.centerY = b / (-2);
		circle.radius = std::sqrt(a * a + b * b - 4 * c) / 2.0;
		return 0;
	}

	/**
	 * @desc 随机拟合线实现
	 */
	template<typename TYPE>
	int ranscFitLineImpl(const std::vector<cv::Point_<TYPE>>& vPoint, Line& line, double maxDis, double breekNumRatio, double minNumRatio)
	{
		if (vPoint.size() <= 3) {
			return -1;
		}

		int loopCount = 50;
		double maxScore = -1.f;
		Line bestLine;

		double score = 0;
		int size = vPoint.size();
		int lastBadCount = size;
		for (int i = 0;i < loopCount;++i) {
			//随机取点
			std::vector<cv::Point_<TYPE>> vSeedPoints = getRandomElementsImpl(vPoint, 2);
			if (vSeedPoints[0] == vSeedPoints[1])
			{
				//取到相同的两个点时，直接下一次
				continue;
			}

			//两点确定一条直线
			Line curline = getLineImpl(vSeedPoints[0], vSeedPoints[1]);
			normalize(curline);			//标准化

			//结果打分
			int badCount = 0;
			for (const cv::Point_<TYPE>& pt : vPoint) {
				//对于标准化的线，|ax+by+c|就是距离
				if (abs(curline.a*pt.x + curline.b*pt.y + curline.c) > maxDis)
				{
					badCount++;
					//坏点个数大于等于最好分数的坏点个数时，直接跳出本次循环
					if (badCount >= lastBadCount) {
						badCount = size;
						break;
					}
				}
			}

			//打分判断
			double thisScore = (size - badCount) * 1.f / size;
			if (thisScore >= breekNumRatio)
			{
				line = curline;
				//std::cout << "max score:" << thisScore << std::endl;
				return 0;
			}

			//高于当前打分，则替换为当前结果
			if (thisScore > maxScore)
			{
				maxScore = thisScore;
				lastBadCount = badCount;
				bestLine = curline;
			}
		}

		//std::cout << "max score:" << maxScore << std::endl;
		//小于最低要求时，返回失败
		if (maxScore < minNumRatio)
		{
			return -1;
		}

		line = bestLine;
		return 0;
	}

	/*
	* @desc 统计点集的范围
	* @param:
	*	-vPoint:待统计的点集
	*	-minX:输出X最小值
	*	-maxX:输出X最大值
	*	-minY:输出Y最小值
	*	-maxY:输出Y最大值
	*/
	template<typename TYPE>
	void staticRangeImpl(const std::vector<cv::Point_<TYPE>>& vPoint, TYPE& minX, TYPE& maxX, TYPE& minY, TYPE& maxY) {
		minX = BIG_VALUE, minY = BIG_VALUE, maxX = SMALL_VALUE, maxY = SMALL_VALUE;
		std::for_each(vPoint.begin(), vPoint.end(), [&minX, &maxX, &minY, &maxY](const cv::Point_<TYPE>& pt) {
			if (pt.x > SMALL_VALUE&&pt.x < BIG_VALUE) {
				maxX = std::max(maxX, (TYPE)pt.x);
				minX = std::min(minX, (TYPE)pt.x);
			}
			if (pt.y > SMALL_VALUE&&pt.y < BIG_VALUE)
			{
				minY = std::min(minY, (TYPE)pt.y);
				maxY = std::max(maxY, (TYPE)pt.y);
			}
		});
	}

	/**
	 * @desc 画线实现
	 */
	template<typename TYPE>
	void drawPointsImpl(cv::Mat& mat, const std::vector<cv::Point_<TYPE>>& vPoint, TYPE minX, TYPE maxX, TYPE minY, TYPE maxY, cv::Scalar color = cv::Scalar(0, 255, 0))
	{
		int iWidth = mat.cols,
			iHeight = mat.rows;
		double ratioX = iWidth *1.0/ (maxX - minX);
		double ratioY = iHeight *1.0/ (maxY - minY);
		//
		for (auto& pt : vPoint) {
			cv::Point p((pt.x - minX)*ratioX, (pt.y - minY)*ratioY);
			cv::circle(mat, p, 1, color, -1, cv::LINE_AA);
		}
	}
	
	/**
	 * @desc ransc通用实现
	 */
	template<typename _DATATYPE,typename _RSTTYPE>
	int ranscCommon(const std::vector<_DATATYPE>& vPoint, _RSTTYPE& line,
		std::vector<_DATATYPE>(fnGetSeekEle)(const std::vector<_DATATYPE>&),
		bool(fnFitTarget)(const std::vector<_DATATYPE>&, _RSTTYPE&),
		bool(fnIsEleGood)(_DATATYPE, _RSTTYPE,double),
		double thre, double breekNumRatio, double minNumRatio)
	{
		if (vPoint.size() <= 10) {
			return -1;
		}

		int loopCount = 50;
		double maxScore = -1.f;
		_RSTTYPE bestLine;

		double score = 0;
		int size = vPoint.size();
		int lastBadCount = size;
		for (int i = 0; i < loopCount; ++i) {
			//随机取点
			std::vector<_DATATYPE> vSeedPoints = fnGetSeekEle(vPoint);
			_RSTTYPE curline;
			if (!fnFitTarget(vSeedPoints,curline)) continue;

			//结果打分
			int badCount = 0;
			for (const auto& pt : vPoint) {
				//对于标准化的线，|ax+by+c|就是距离
				if (!fnIsEleGood(pt,curline,thre))
				{
					badCount++;
					//坏点个数大于等于最好分数的坏点个数时，直接跳出本次循环
					if (badCount >= lastBadCount) {
						badCount = size;
						break;
					}
				}
			}

			//打分判断
			double thisScore = (size - badCount) * 1.f / size;
			if (thisScore >= breekNumRatio)
			{
				line = curline;
				return 0;
			}

			//高于当前打分，则替换为当前结果
			if (thisScore > maxScore)
			{
				maxScore = thisScore;
				lastBadCount = badCount;
				bestLine = curline;
			}
		}

		//小于最低要求时，返回失败
		if (maxScore < minNumRatio)
		{
			return -1;
		}

		line = bestLine;
		return 0;

	}

};
